#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/common.h>
#include <iostream>
#include <thread>
#include <chrono>

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudNormalT;

int main(int argc, char** argv) {
    std::cout << "Starting multi-point cloud visualization..." << std::endl;

    // 创建PCL可视化器
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("Multi Point Cloud Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addCoordinateSystem(20.0);
    viewer->initCameraParameters();

    // 定义要加载的点云文件路径
    std::vector<std::string> cloud_files = {
        "filtered.pcd",
        "gluing_path_0.pcd", 
        "gluing_paths_all.pcd"
    };

    // 定义颜色
    std::vector<std::array<int, 3>> colors = {
        {128, 128, 128},  // 灰色 - 滤波后的原始点云
        {0, 255, 0},      // 绿色 - 第一条打胶路径
        {255, 0, 0}       // 红色 - 所有打胶路径
    };

    // 定义点云名称
    std::vector<std::string> cloud_names = {
        "filtered_cloud",
        "path_0_cloud", 
        "all_paths_cloud"
    };

    // 定义点云显示名称
    std::vector<std::string> display_names = {
        "Filtered Point Cloud",
        "Gluing Path 0",
        "All Gluing Paths"
    };

    int loaded_clouds = 0;

    // 加载并显示所有点云
    for (int i = 0; i < 3; ++i) {
        PointCloudT::Ptr cloud(new PointCloudT);
        
        if (pcl::io::loadPCDFile<PointT>(cloud_files[i], *cloud) == -1) {
            std::cerr << "Warning: Couldn't read file " << cloud_files[i] << std::endl;
            continue;
        }

        std::cout << "Loaded " << cloud->size() << " points from " << cloud_files[i] << std::endl;

        // 添加点云到可视化器
        pcl::visualization::PointCloudColorHandlerCustom<PointT> color_handler(
            cloud, colors[i][0], colors[i][1], colors[i][2]);
        viewer->addPointCloud<PointT>(cloud, color_handler, cloud_names[i]);
        
        // 设置点大小 - 路径点云放大方便观察
        int point_size;
        if (i == 0) {
            point_size = 1;  // 滤波后的点云较小
        } else {
            point_size = 8;  // 路径点云放大方便观察
        }
        viewer->setPointCloudRenderingProperties(
            pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, cloud_names[i]);

        loaded_clouds++;
    }

    if (loaded_clouds == 0) {
        std::cerr << "Error: No point clouds could be loaded!" << std::endl;
        return -1;
    }

    std::cout << "\nLoaded " << loaded_clouds << " point clouds successfully!" << std::endl;
    
    // 设置相机视角
    viewer->setCameraPosition(0, 0, 100, 0, 0, 0, 0, 1, 0);

    // 添加文本说明
    std::string info_text = "Gluing Path Point Clouds:\n";
    for (int i = 0; i < static_cast<int>(display_names.size()) && i < loaded_clouds; ++i) {
        info_text += "- " + display_names[i] + "\n";
    }
    info_text += "\nControls:\n";
    info_text += "- Mouse: Rotate view\n";
    info_text += "- Mouse wheel: Zoom\n";
    info_text += "- Right click + drag: Pan\n";
    info_text += "- Press 'q' to quit\n";
    info_text += "- Press '1'-'3' to toggle clouds";

    viewer->addText(info_text, 10, 10, 12, 1.0, 1.0, 1.0, "info_text");

    // 键盘回调函数
    std::cout << "\nVisualization Controls:" << std::endl;
    std::cout << "- Mouse: Rotate view" << std::endl;
    std::cout << "- Mouse wheel: Zoom in/out" << std::endl;
    std::cout << "- Right click + drag: Pan view" << std::endl;
    std::cout << "- Press '1' to toggle filtered point cloud" << std::endl;
    std::cout << "- Press '2' to toggle gluing path 0" << std::endl;
    std::cout << "- Press '3' to toggle all gluing paths" << std::endl;
    std::cout << "- Press 'q' to quit" << std::endl;

    // 显示循环
    while (!viewer->wasStopped()) {
        viewer->spinOnce(60000);
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    std::cout << "Visualization ended." << std::endl;
    return 0;
}
